#include <stdlib.h>
#include <math.h>
#include "rtthread.h"
#include "openart_1.h"
#include "ch_tsp.h"


int Graph[SIZE][SIZE];

/* path from 1 to SIZE */
int Path[SIZE];

int point[SIZE][2];

void bubble()
{
    int i,j,temp[2];
    for(i=1;i<=SIZE-1;i++)
        for(j=0;j<SIZE-i;j++)
    {
        if(point[j][0]>point[j+1][0])
        {
            temp[0]=point[j][0];
            temp[1]=point[j][1];
            point[j][0] = point[j+1][0];
            point[j][1] = point[j+1][1];
            point[j+1][0] = temp[0];
            point[j+1][1] = temp[1];
        }
        else if(point[j][0] == point[j+1][0])
        {
            if(point[j][1] > point[j+1][1])
            {
                temp[0] = point[j][1];
                point[j][1] = point[j+1][1];
                point[j+1][1] = temp[0];
            }
        }
    }
}

void InitPath (void)
{
    for(int i = 0;i < SIZE;i++)
        Path[i] = i;
}

void InitGraph (void)
{
		bubble();
    for(int i = 0;i < SIZE;i++){
        for(int j = i+1;j < SIZE;j++)
        {
            Graph[j][i] = lround(sqrt(pow(point[j][0] * 20- point[i][0] * 20,2) + pow(point[j][1] * 20 - point[i][1] * 20,2)));
            Graph[i][j] = Graph[j][i];
        }
        Graph[i][i] = 0;
    }
    InitPath();
}

void PrintGraph (void)
{
    for(int i = 0;i < SIZE;i++)
    {
        for(int j = 0;j < SIZE;j++)
            rt_kprintf("%d\t",Graph[i][j]);
        rt_kprintf("\n");
    }
}

int ScorePath (void)
/* score for the current path */
{
    int score = 0;
    for(int i = 0;i < SIZE-1;i++)
        score += Graph[Path[i]][Path[i+1]];
    score += lround(sqrt(pow(point[Path[SIZE-1]][0] * 20 - 35 * 20, 2) + pow(point[Path[SIZE-1]][1] * 20 - 25 * 20, 2))); 
    score += lround(sqrt(pow(point[Path[0]][0] * 20, 2) + pow(point[Path[0]][1] * 20, 2))); 
    return score;
}

void swap (int *a, int *b)
{
    int temp = *a;
    *a = *b;
    *b = temp;
}

int ClimbStep (void)
/* climb for a step */
{
    int MinScore = 100000, CurScore, chg_a, chg_b;
    for(int i = 0;i < SIZE;i++)
        for(int j = i+1;j < SIZE;j++)
        {
            swap(&Path[i],&Path[j]);
            CurScore = ScorePath();
            if(CurScore < MinScore){
                MinScore = CurScore;
                chg_a = i;
                chg_b = j;
            }
            swap(&Path[i],&Path[j]);
        }
    swap(&Path[chg_a],&Path[chg_b]);
    return MinScore;
}

int ClimbHill (void)
/* climb the mountain */
{
    int MinScore = 100000, CurScore;
    while((CurScore = ClimbStep()) < MinScore)
        MinScore = CurScore;
    return MinScore;
}

//int main (void)
//{
//    InitGraph();
//    PrintGraph();
//    printf("\n%d\n",ClimbHill());
//    for(int i = 0;i < SIZE;i++)
//        printf("%d ",Path[i]);
//    printf("\n");
//    for(int i = 0;i < SIZE;i++)
//        printf("%d   %d\n",point[Path[i]][0], point[Path[i]][1]);
//}